
void LivingSpot::standBy() {
  
  //moveSpot(CENTER_AD_MIRROR_X,CENTER_AD_MIRROR_Y);
  computeNormalVector(Az, El, Aper_DA);
    
    // better not to use the potentiometers, because we may use them to "store" good values of treshold
  // Az=(xMax-xMin)*IO.readPotA()+xMin; 
  // El=(yMax-yMin)*IO.readPotB()+yMin; 
   
   Az=0.5*(xMax-xMin)+xMin-2.25*Aper_DA;  // this is specially tailored for three letters ("SLP")
   El=0.5*(yMax-yMin)+yMin-0.5*Aper_DA;
   updatePosition(Az+identifier*1.5*Aper_DA, El); // space between letters is 1.5 their size
 
  // spotArray[0].checkRecenteringVector(); 
}


void LivingSpot::tracking() {
 // if (numSpots>1) computeNormalVector(Az, El, Aper_DA);
	if (computeNormalVector(Az, El, Aper_DA)){
         updatePosition(Az + 0.8*deltaAz, El + 0.8*deltaEl);	
} else { // search for something? or stay there, or move to a central position?
}
}

void LivingSpot::showLetters() { // similar to computeNormalVector, but wihout additional points for the saccade

	// First, perform the scanning saccade (without the additional points for the circular saccade)
	 scanTrajectory_Letters(_circularSaccade, _numSamplesCircularSaccade, Az, El, Aper_DA, 0);//paramRotation);  // perform a CIRCULAR saccade and acquire data

	// Then, analyze the data (note: data is ALWAYS on the array _rawScanData
	analyzeScanData(_numSamplesCircularSaccade); //_numSamplesCircularSaccade is the current size of the circular saccade (in the future, we will have a class "trajectory"
	// that will have a member num_samples.

	// We don't need to compute angular correction; by the way, since we display alphanumeric data on Az+nx, El+ny...
        // we better fix these values here: 
        //deltaAz=0; deltaEl=-2*Aper_DA;
        nx=0; ny=1;
          
   // POSITION:
   Az=(xMax-xMin)*IO.readPotA()+xMin; 
   El=(yMax-yMin)*IO.readPotB()+yMin; 
   
   updatePosition(Az+identifier*2*Aper_DA, El);
}

// Refraction following Snell Law:
int  LivingSpot::lightRefraction() { 
	//static int countsign=1;
	//static float n1; // this way the variable does not need to be declared global
	float alpha1, alpha2;
	float v2x, v2y=0;
	float v1x, v1y;
	float vnewx, vnewy;
	float rotx, roty;
	float normSpeed=sqrt(vx*vx+vy*vy); // this is conserved, but normally it should change with the index!!!

        IO.setRedPower_mW(RedLaserPower); // in mW
        IO.setGreenPower(GreenLaserPower);

	//_averageLockIn = _avLockInTab[0];
	//for(int i=1; i<_nbAvr; i++) {_averageLockIn += _avLockInTab[i];}
	//_averageLockIn /= _nbAvr;

	// (1) First, do a readout of the lockin data:
#ifdef _NOHARD
	float currLockIn =test_noHardware_PD(Az, El);
#else
	float currLockIn = 0;
	//moveSpot(Az, El);
	for (int i=0; i<3; i++) currLockIn+= IO.LockInRead_Volts();
	currLockIn/=3.0;
#endif

	float n2=refractionIndex(currLockIn); // this is the calculation of the index of refraction from the intensity
	if (firstTimeStartRefraction) {
		n1=n2;            
		firstTimeStartRefraction=false;
		//El_Default=El_Default+countsign*25; if (El_Default>2450) countsign*=-1; if (El_Default<550) countsign*=-1; //Az_Default=550; };
		El_Default=El_Default+countsign*50;
                if (El_Default>CENTER_AD_MIRROR_Y+300) countsign*=-1;
                if (El_Default<CENTER_AD_MIRROR_Y-300) countsign*=-1; //Az_Default=550; };
	}
	if (abs(n1-n2)>.1) {

		// this is for activating the green laser at the transition:
		collision=true; 

		//goWhite();
		//spot.moveSpot(int(spot.getAz() - spot.vx), int(spot.getEl() - spot.vy));

                moveSpot(Az-vx, El-vy);
		
                //Az=Az-vx;
		//El=El-vy;
		//IO.setMirrorsXY_AD(int (Az), int (El));
		//delayMicroseconds(100);
		//moveSpot(Az-0.5*vx, El-0.5*vy);

		if (computeNormalVector(Az, El, Aper_DA)){ 

			v1x=(nx * vx + ny * vy); // rem: norm of (nx, ny) is 1
			if (v1x<0) {// this means that n and v point to different directions with respect to the interface
				nx=-nx; ny=-ny; 
				v1x=-v1x;  
			}

			// compute +90 deg rotated normal vector forming orthonormal base with (nx, ny):
			rotx=-ny; 
			roty=nx;
			v1y=(rotx * vx + roty * vy);

			// compute incident angle alpha1:
                        if (normSpeed>0) alpha1=asin(v1y/normSpeed); // att: normSpeed must be > 0 (this should be almost the case... BUT CHECK THIS!!! there may become 0 with bouncing...)
                         else alpha1=1;// can be anything.. it's meaningles...

			// Two cases: TIR or normal refraction:
			if (abs(sin(alpha1))<(1.0*n2/n1)) { // this means normal refraction
				alpha2=asin(1.0*n1/n2*sin(alpha1));
				v2x=cos(alpha2)*normSpeed;
				v2y=sin(alpha2)*normSpeed;
			} else { // this means total internal reflexion (TIR):
				v2x=-v1x;
				v2y=v1y;
			}

			// finally, reconstruct vector v2 in mirror X and Y coordinates:
			vnewx=v2x*nx + rotx*v2y;// * rotx;
			vnewy=v2x*ny + roty*v2y;// * roty;

			moveSpot(Az+2*vnewx, El+2*vnewy); 
			//moveSpot(Az -vx+vnewx, El - vy+vnewy); // the factor on the speed is for going quickly on the other side of the interface
			vx=vnewx; vy=vnewy;
		}
	} else { // this means that indexes are too closr (their difference may be just a matter of noise)
		moveSpot(Az+ vx, El+ vy); // rectilinear motion (rem: could be bent by "gravitanional forces" using newtonian deltaElnamics...)
		//if(_idAv == _nbAvr) _idAv = 0;
		//_avLockInTab[_idAv] = currLockIn;
		//_idAv++;	

		collision=false; 
	}

	// save "old" value of index:
	n1=n2;
	//delay(10);
}

float  LivingSpot::refractionIndex(float intensity) {
	//if (intensity>(MAX_INTENSITE- MIN_INTENSITE)*.5+MIN_INTENSITE) return(1); else return(2);
	//  if (intensity>(MAX_INTENSITE- MIN_INTENSITE)*.5+MIN_INTENSITE) return(1); else return(2);

	// for white background:
	if (intensity>fixed_Threshold_Volts) return(1.7); else return(1);
	// for black background:
	//if (intensity>fixed_Threshold_Volts) return(1); else return(1.7);//fixed_Threshold_Volts

        //if (intensity>auto_Threshold_Volts) return(1); else return(1.7);


	//        if (intensity<fixedThreshold_Volts) return(1); 
	//        else if (intensity<fixedThreshold_Volts2) return(1.6);
	//        else return(100);

	// return(1.0*intensity/750+1);
}


//draw a line until it enter in a dark area, compute the reflexion vector using the normal vector of the contour
//return 1 if bounce on a dark area, return 0 if it go straight
int LivingSpot::fastBouncing(){ 

	// the following line is just to slow down in case we use processing to check the output of the mirrors: 
#ifdef _NOHARD
	computeNormalVector(Az, El, Aper_DA);
#endif

	_averageLockIn = _avLockInTab[0];
	for(int i=1; i<_nbAvr; i++) {_averageLockIn += _avLockInTab[i];}
	_averageLockIn /= _nbAvr;


#ifdef _NOHARD
	int currLockIn =test_noHardware_PD(Az, El);
#else
	int currLockIn = IO.LockInRead_Volts();
#endif

	if (currLockIn < _averageLockIn / thresholdFactorFB) {

		//TO TUNE:
		//go back before performing a saccade (depend of the speed)
		moveSpot(Az - vx, El - vy);
		//delayMicroseconds(100);

		if(computeNormalVector(Az, El, Aper_DA)){

			float norma=(nx * vx + ny * vy);

			vx = -2.0 * norma * nx + vx;
			vy = -2.0 * norma * ny + vy;

			moveSpot(Az + vx + deltaAz, El + vy + deltaEl);

			return 1;

		}
		//else ??
		return 1;

	} 
	else {
		moveSpot(Az + vx, El + vy);
		if(_idAv == _nbAvr) _idAv = 0;
		_avLockInTab[_idAv] = currLockIn;
		_idAv++;
		return 0; 
	}
}


//scan and bounce on dark edge, by just changing the direction of the HORIZONTAL SPEED: 
int LivingSpot::scanBouncing(){ 

	_averageLockIn = _avLockInTab[0];
	for(int i=1; i<_nbAvr; i++) {_averageLockIn += _avLockInTab[i];}
	_averageLockIn /= _nbAvr;

#ifdef _NOHARD
	int currLockIn =test_noHardware_PD(Az, El);
#else
	int currLockIn =IO.LockInRead_Volts();
#endif

	if (currLockIn < _averageLockIn / thresholdFactorFB) {
		vx *= -1.0;
		moveSpot(Az + vx, El + vy);
		return 1;

	} 
	else {
		moveSpot(Az + vx, El);

		if(_idAv == _nbAvr) _idAv = 0;
		_avLockInTab[_idAv] = currLockIn;
		_idAv++;
		return 0; 
	}
}


//normal bouncing, make a saccade even if nothing
//return 1 if bounce on a dark area, return 0 if it go straight
int LivingSpot::simpleBouncing(int numSpots){ 
	//if (numSpots>1) computeNormalVector(Az, El, Aper_DA);

        if (vx>35) vx=35;  if (vx<-35) vx=-35;
        if (vy>35) vy=35;  if (vy<-35) vy=-35;
        
	if (computeNormalVector(Az, El, Aper_DA)){

		float norma=(nx * vx + ny * vy);

		vx = 1.3*(-2.0 * norma * nx + vx); // the factor is for adding energy when bouncing. 
		vy = 1.3*(-2.0 * norma * ny + vy);

		// here, we use update position instead of moveSpot, because we want to avoid a line between the last saccade point and the CENTER of the next saccade...
		updatePosition(Az + deltaAz + vx, El + deltaEl + vy);		
		return 1;

	} 
	else {
		updatePosition(Az + vx, El + vy);
		return 0; 
	}
}

int LivingSpot::simpleBouncingCentralForce(int numSpots){ 
	//if (numSpots>1) computeNormalVector(Az, El, Aper_DA);
      
        if (vx>30) vx=30;  if (vx<-30) vx=-30;
        if (vy>30) vy=30;  if (vy<-30) vy=-30;
        

	if (computeNormalVector(Az, El, Aper_DA)){

		float norma=(nx * vx + ny * vy);

		vx = 1.2*( -2.0 * norma * nx + vx); // the factor is to give some energy...
		vy = 1.2*(-2.0 * norma * ny + vy);

		// here, we use update position instead of moveSpot, because we want to avoid a line between the last saccade point and the CENTER of the next saccade...
		updatePosition(Az + deltaAz + vx, El + deltaEl + vy);		
		return 1;

	} 
	else {
           // Central force:
            float mx=Az-CENTER_AD_MIRROR_X, my=El-CENTER_AD_MIRROR_Y;
           float dist = sqrt(mx*mx + my*my+0.1);
           vx=vx-0.000005*mx*dist-vx*0.001; // spring central force and viscosity 
           vy=vy-0.000005*my*dist-vy*0.001; 
       
		updatePosition(Az + vx, El + vy);
		return 0; 
	}
}



// Update spot position and kinematic parameters using Newton dynamics:
int  LivingSpot::newtonDynamics(int numSpots){ // rem: we could have dt as a time increment parameter, but we would need a precision of microseconds. I leave it for now...

	totalForceX=0; totalForceY=0;

	// compute dt in the loop:
	if (micros()>oldTime)  dt=0.001*(micros()-oldTime); // dt in MILLI-SECONDS!
	oldTime=micros();
	//Serial.println(dt); // it turns out that dt is about 5 ms
	dt*=slowTime;
        
        // for tests: 
        dt=0.01;

	// We don't need the speed for integrating using the VERLET algorithm, but we may need it for sending data for sound, or if we compute the viscosity force
	// non explicitly (meaning, not integrating it on the verlet formula):
	vx=(Az-Az_old)/dt;
	vy=(El-El_old)/dt;

	float distCenter=sqrt((Az-CENTER_AD_MIRROR_X)*(Az-CENTER_AD_MIRROR_X)+(El-CENTER_AD_MIRROR_Y)*(El-CENTER_AD_MIRROR_Y))+10; // + somthing to avoid singularities

	//if (numSpots>1) computeNormalVector(Az, El, Aper_DA);
	boolean touch=computeNormalVector(Az, El, Aper_DA);

	//(1) bouncing on contours (and mirror limits?):
	if (bouncingForceMode==true) {
		if (touch) { // if something was in the way of the spot, 
			// compute the bouncing force as a function of the recentering vector:
			// (nx, ny) (normed), or (deltaAz, deltaEl), whose norm is NormVector,
			//  everything weighed by the boucingForceFactor:

			/*
			// TEST: force equal to repulsive force in case of interparticle interaction, with the norm of the recentering 
			// vector representing the proximity of the "two particles":
			float daux=NormVector/Aper_DA*rangeInteractionShort/1.414; 
			daux=interForceFactor*(2.0/(rangeInteractionShort*rangeInteractionShort)-1.0/(daux*daux));
			// Note: the meaning of this, is that when NormVector=Aper_DA (meaning half of the saccade is inside a black zone), then the
			// repulsive force felt correspond to that of two particles of charge=1 in the exact location when their interaction force is 0 - i.e., their 
			// distance is rangeInteractionShort/sqrt(2). NormVector is always < Aper_DA, so the force is always "repulsive" (pointing towards the 
			// white zone). The bouncingForceFactor is an additional parameter.
			totalForceX=-nx*daux*bouncingForceFactor;
			totalForceY=-ny*daux*bouncingForceFactor;
			*/
			totalForceX=-nx*bouncingForceFactor; // we can use the normalized nx or deltaAz
			totalForceY=-ny*bouncingForceFactor; // we can use the normalized nx or deltaEl
		}
	}

	// (1.b) bouncing on countours by inverting speed:
	if (bouncingSpecularMode) {
		if(touch){ // if something was in the way of the spot             
			// Az_old is the current position, Az is the past position. We need to reverse them:
			float auxX=Az; Az=Az_old; Az_old=auxX;
			float auxY=El; El=El_old; El_old=auxY;          
		}
	}


	// (2) particle interaction force:
	if (particleInteractionMode==true) {
		totalForceX+=interForceX*interForceFactor;
		totalForceY+=interForceY*interForceFactor; // the force produced by interaction with other particles
	}

	// (3) Central force (SPRING!!):
	if (centralForceMode==true) {
		totalForceX-=centralForceFactor*(Az-CENTER_AD_MIRROR_X); 
		totalForceY-=centralForceFactor*(El-CENTER_AD_MIRROR_Y); 
	}

	// (4) parallel force:
	if (parallelForceMode==true) {
		totalForceX+=gx*mass;
		totalForceY+=gy*mass; 
	}

	//(5) viscosity force (for verlet integration this could be done in the integration formula...):
	if (visForceMode==true) {
		// REM: if visForceFactor is the fraction of the speed lost to friction (no unit) used in the verlet formula, then the 
		// factor that we need to use here is: visForceFactor*mass/dt (unit is kg/sec):
		totalForceX-=visForceFactor*vx*mass/dt;
		totalForceY-=visForceFactor*vy*mass/dt;	
	}

	// Compute the acceleration at time t from the interaction forces (at time t): 
	ax=totalForceX/mass;
	ay=totalForceY/mass;

	// INTEGRATION USING VERLET ALGORITHM: 

	//First, compute the new position using the two previous positions, plus the current acceleration (computed from the CURRENT position): 
	// NOTE: definitions are as follows: Az2_AD=x(t+dt), Az=x(t) and Az_old=x(t-dt)
	// With viscosity in the formula:
	// REM: visForceFactor is the fraction of the speed lost to friction (no unit!)
	//float Az2_AD=(2-auxVerletVisc)*Az-(1-auxVerletVisc)*Az_old+ax*dt*dt; 
	//float El2_AD=(2-auxVerletVisc)*El-(1-auxVerletVisc)*El_old+ay*dt*dt;

	float Az2_AD=2*Az-Az_old+ax*dt*dt;
	float El2_AD=2*El-El_old+ay*dt*dt;

	// save the position at time t for the new iteration as the "old" position t-dt:
	Az_old=Az; El_old=El;

	// update the new position (rem: since we are using a scanning saccade per iteration, we don't want the laser to position itself in 
	// the center of the saccade; then, call updatePosition instead of moveSpot():
	updatePosition(Az2_AD, El2_AD); 

	// reset interforce for THIS particle:
	interForceX=0; interForceY=0;
}

// New (1.3.2010): need to use a method to set the speed when in newtonian deltaElnamics, because I am using the verlet integrator:
void LivingSpot::updateSpeed(float speedX, float speedY){
	vx=speedX; 
	vy=speedY;
	//if (behaviourMode==NEWTON_DYNAMICS) {
	//(in fact, no need to check that because the other modes don't use Az_old and El_old)
	// rem: I assume a dt equal to 5ms/6 (see newtonDynamics())
	Az_old=Az-2.0*dt*slowTime*speedX;
	El_old=El-2.0*dt*slowTime*speedY;
	//}
}     

int LivingSpot::computeInteraction(LivingSpot* spotArray, int numSpots) {
	// rem: the total number of spots is: numberOfSpots, but some may not be active (alive mode)
	// the currect spot ("this") has id: identifier (a number from 0 to numberOfSpots)
	collision=false; //initialize collision flag (rem: collision will take place when in REPULSIVE distance)
	if (alive==true) {
		for (int i=identifier+1; i<numSpots; i++) {
			if (spotArray[i].alive==true) { // else, do nothing (just skip this spot)
				float ux, uy, sqinterSpotDist, interSpotDist;
				ux=spotArray[i].Az-Az;
				uy=spotArray[i].El-El;
				interSpotDist=sqrt(ux*ux+uy*uy)+6; // to avoid singularities

				if (interSpotDist<rangeInteractionLong) { //otherwise do nothing
					// REM: force here can be on 1/r^2, 1/r or proportional to the distance...
					// (a) in 1/r^2:
					float interForce=spotArray[identifier].charge*spotArray[i].charge/(interSpotDist*interSpotDist*interSpotDist); // *interForceFactor is done at the update function
					// (b) in 1/r:
					//float interForce=spotArray[identifier].charge*spotArray[i].charge/(interSpotDist*interSpotDist); // *interForceFactor is done at the update function
					// (c)  proportional to r (spring like):
					//float interForce=spotArray[identifier].charge*spotArray[i].charge*(interSpotDist-rangeInteractionShort); // *interForceFactor is done at the update function

					if (interSpotDist<rangeInteractionShort) {  // this means repulsive force, and "collision"
						collision=true;
						// (a) in 1/r^2:
						interForce=2.0/(rangeInteractionShort*rangeInteractionShort*rangeInteractionShort)-interForce;
						// (b) in 1/r:
						//interForce=2.0/(rangeInteractionShort*rangeInteractionShort)-interForce;
						// (c) proportional to r (spring like): nothing needs to be done
						// ...

					} else { // this means attractive force, but not collision
						// interForce*=1.0; // nothing is needed
					}
					// now, compute the X and Y components and add to the forces on THIS particle, and on the other particle:
					interForceX+=interForce*ux;
					interForceY+=interForce*uy;

					spotArray[i].interForceX-=interForce*ux;
					spotArray[i].interForceY-=interForce*uy;
				} 
			}
		}
	}
	return(collision);
}


// ====================================================================================================================================================

//follow a contour; compute the correction vector to stay on a contour
//return true if capable to follow, return false if the contour is lost
boolean LivingSpot::moveAlongContour(int numSpots){
	boolean returnValue=false;
	//move to the first position of the saccade:
	//Spi.writeOutX(int(Az + _saccadePosArray[0]));
	//Spi.writeOutY(int(El + _saccadePosArray[1]));
	//delayMicroseconds(1000);

	//perform a saccade:

	if (computeNormalVector(Az, El, Aper_DA)){ // this means that the saccade is on something interesting
		
// EXPERIMENTAL: draw something on the contour: 
//IO.setGreenPower(HIGH);
//for (int i=0; i<4; i++) {
//moveUncheckedSpot(Az,El); 
//moveUncheckedSpot(Az+500*nx, El+500*ny); 
//}
//IO.setGreenPower(LOW);

                returnValue=true;

		float normAuxVector = speedContourFollowing * Aper_DA;

		//Then, compute the 90CCW vector with respect to (nx, ny), which is the normalized version of the correction 
		// vector (deltaAz,deltaEl), whose norm is NormVector. 
		// but using instead normAuxVector (actually, we could do  px= -1.0 * deltaAz * speedContourFollowing directly)
		float px= -1.0 * ny * normAuxVector;
		float py= 1.0 * nx * normAuxVector;

		// Second, adjust the correcting vector so as to mantain penetration on the countour: 
		// this part takes the correction vector (deltaAz, deltaEl) computed in angular correction, and readjust it so as to maintain a "fixed penetration" on the 
		// lighted zone. The fixed penetration length is equal to fixedPenetration=penetrationFactor*saccadeDiam;
		float fixedPenetration = 1.0 * Aper_DA;
		float correctingFactor = 0.5 * (NormVector - fixedPenetration); // can be negative or positive. By putting <1, we "converge" smoothly to the right position

		int deltaAzx = nx * correctingFactor + px;
		int deltaEly = ny * correctingFactor + py;

		// Finally, update position of center of saccade by adding pVector and dVector togheter: 
		updatePosition(Az + deltaAzx, El + deltaEly); 
	}

	return(returnValue);
} 

boolean LivingSpot::searchContour(int numSpots) {
	// First, search for something with a radius proportional to the search cycle (attemptsSearching_counter):
	//if(numSpots>1) computeNormalVector(Az, El, Aper_DA*(attemptsSearching_counter*attemptsSearching_counter/10+0.5));
        float auxRadius=Aper_DA*(attemptsSearching_counter*attemptsSearching_counter/10+1); // this means that when attemptsSearching_counter=0, radius is Aper_DA, and when attemptsSearching_counter=10, radius is 10*Aper_DA
	if(onContour=computeNormalVector(Az, El,auxRadius)) { 
		if (whiteProportion<0.4) 
			updatePosition(Az + 0.5*auxRadius*nx, El + 0.5*auxRadius*ny); 
		else 
			updatePosition(Az -0.5*auxRadius*nx, El - 0.5*auxRadius*ny); 
		attemptsSearching_counter=0;
		return(true);  
	} 
	return(false);
}


// Miscelanea behaviours: ==================================================================

/*
//draw the letters SLP horizontaly
//possibility to change the position and the size on X and y axis
//nloops is the number of time it will draw the logo
void LivingSpot::drawSLPLogo(int centerLogo_X, int centerLogo_Y, int sizeLogo_X, int sizeLogo_Y, int nloops){
	for(int n=0; n<nloops; n++){
		for(int i=tableSLPLogo_size-1; i>=0; i--){
			moveSpot( centerLogo_X + tableSLPLogo_X[i] * sizeLogo_X, centerLogo_Y + tableSLPLogo_Y[i] * sizeLogo_Y);
			delayMicroseconds(84);
		}
		for(int i=0; i<tableSLPLogo_size; i++){
			moveSpot( centerLogo_X + tableSLPLogo_X[i] * sizeLogo_X, centerLogo_Y + tableSLPLogo_Y[i] * sizeLogo_Y);
			delayMicroseconds(84);
		}
	}
}

//draw the letters SLP oriented with an angle in deg
//possibility to change the position and the size on X and y axis
//nloops is the number of time it will draw the logo
void LivingSpot::drawSLPLogoAngle(float angleLogo, int centerLogo_X, int centerLogo_Y, int sizeLogo_X, int sizeLogo_Y, int nloops){
	for(int n=0; n<nloops; n++){
		float ang = angleLogo * 0.01745;
		float cosAngX = cos(ang) * sizeLogo_X;
		float sinAngX = sin(ang) * sizeLogo_X;
		float cosAngY = cos(ang) * sizeLogo_Y;
		float sinAngY = sin(ang) * sizeLogo_Y;
		for(int i=tableSLPLogo_size-1; i>=0; i--){
			moveSpot( centerLogo_X + tableSLPLogo_X[i] * cosAngX + tableSLPLogo_Y[i] * sinAngY , centerLogo_Y - tableSLPLogo_X[i] * sinAngX + tableSLPLogo_Y[i] * cosAngY);
		}
		for(int i=0; i<tableSLPLogo_size; i++){
			moveSpot( centerLogo_X + tableSLPLogo_X[i] * cosAngX + tableSLPLogo_Y[i] * sinAngY , centerLogo_Y - tableSLPLogo_X[i] * sinAngX + tableSLPLogo_Y[i] * cosAngY);
		}
	}
}

//draw the letters SLP and make it doing a complete 360 deg loop
//possibility to change the position, the size on X and Y axis and the speed of rotation
void LivingSpot::drawSLPLogoRotating(int speedRotation, int centerLogo_X, int centerLogo_Y, int sizeLogo_X, int sizeLogo_Y){
	for(int a = 0; a<=360; a+=speedRotation){
		drawSLPLogoAngle(a, centerLogo_X, centerLogo_Y, sizeLogo_X, sizeLogo_Y, 1);
	}
}
*/

// ========================  CHECKING FUNCTIONS ===============================

// for calibration of delay mirrors, let's do a visible check of the delay:
void LivingSpot::checkRecenteringVector(float Azp, float Elp, float Aperp) {
	if (computeNormalVector( Azp, Elp, Aperp)) {
		IO.setGreenPower(HIGH);
              for (int i=0; i<4;i++) {
			moveUncheckedSpot(Azp, Elp); 
                        delayMicroseconds(300); 
                        moveUncheckedSpot( Azp+3*Aperp*nx,  Elp+3*Aperp*ny);
                         delayMicroseconds(300);
		}
                IO.setGreenPower(LOW);
      }
}

//draw a square corresponding to the maximum area.
//nloops is the number of time it will draw the square.
void LivingSpot::drawMaxArea(int nloops){
	int i, idloop;	

	for(idloop=0; idloop < nloops; idloop++){  
		for(i=yMin; i<yMax; i+=50){
			IO.setMirrorX_AD(xMin);
			IO.setMirrorY_AD(i);
		}
		IO.setMirrorX_AD(xMin);//Spi.writeOutX(xMin);
		IO.setMirrorY_AD(yMax);
		delayMicroseconds(100);
		for(i=xMin; i<xMax; i+=50){
			IO.setMirrorX_AD(i);
			IO.setMirrorY_AD(yMax);
		}
		IO.setMirrorX_AD(xMax);
		IO.setMirrorY_AD(yMax);
		delayMicroseconds(100);
		for(i=yMax; i>=yMin; i-=50){
			IO.setMirrorX_AD(xMax);
			IO.setMirrorY_AD(i);
		}
		IO.setMirrorX_AD(xMax);
		IO.setMirrorY_AD(yMin);
		delayMicroseconds(100);
		for(i=xMax; i>=xMin; i-=50){
			IO.setMirrorX_AD(i);
			IO.setMirrorY_AD(yMin);
		}
		IO.setMirrorX_AD(xMin);
		IO.setMirrorY_AD(yMin);
		delayMicroseconds(100);
	}
} 





